#include "Kinematics.h"
#include <math.h>
#include "Point.h"

#include <stdio.h>


Kinematics::Kinematics(void)
{
}

Kinematics::Kinematics(float d0, float d1, float a2, float d4)
{
	this->d0 = d0;
	this->d1 = d1;
	this->a2 = a2;
	this->d4 = d4;
}

Kinematics::~Kinematics(void)
{
}

Joint Kinematics::inverse(Point p)
{
	/*
	printf("p.x: %f \n", p.getValue(Points::X));
	printf("p.y: %f \n", p.getValue(Points::Y));
	printf("p.z: %f \n", p.getValue(Points::Z));
	*/

	Point o = Point(d1, 0, d0);
	/*
	printf("o.x: %f \n", o.getValue(Points::X));
	printf("o.y: %f \n", o.getValue(Points::Y));
	printf("o.z: %f \n", o.getValue(Points::Z));
	*/	

	float w = p.distance(o);
	//printf("w: %f \n", w);
	float tetaw = atan2(p.getValue(Point::Z)-d0, p.getValue(Point::Y)) + (M_PI/2);
	//printf("tetaw: %f \n", tetaw);

	float ctetak = (pow(d4, 2) - pow(w, 2) - pow(a2, 2))/(-2*w*a2);
	float stetak = sqrt(1-pow(ctetak, 2));
	float tetak = atan2(stetak, ctetak);
	//printf("tetak: %f \n", tetak);

	float teta1 = tetaw - tetak;
	//printf("teta1: %f \n", teta1);

	float teta2 = atan2(p.getValue(Point::X)-d1, sqrt(pow(p.getValue(Point::Z)-d0, 2) + pow(p.getValue(Point::Y), 2)));
	//printf("teta2: %f \n", teta2);

	float cteta3 = ((w*ctetak) - a2)/d4;
	float steta3 = sqrt(1- pow(cteta3, 2));
	float teta3 = atan2(steta3, cteta3);
	//printf("teta3: %f \n", teta3);
	
	//Joints j = Joints(teta1*180/M_PI, teta2*180/M_PI, teta3*180/M_PI);
	Joint j = Joint(teta1, teta2, teta3);

	return j;
}